#include "Utilities.h"
#include <math.h>

namespace SD_Tools
{
	//************************************
	// Method:    DetourFunc
	// Profit:	  Detours a function
	// FullName:  SD_Tools::Internal::DetourFunc
	// Returns:   void*
	// Notes:
	// -Replaces the first 5 Bytes of a function with a jump to your function (0xE9 XXXXXXXX), left over Bytes will be Nopped (0x90)
	// -Return Value is a Pointer to a trampolin with the saved code
	// -len must be atleast 5 and must stop exactly on the end of an assembler code at the beginning of the to detour function
	// -since it does a code change, easily detected by most anticheats
	//************************************
	void* DetourFunc( PBYTE src, const PBYTE dst, const int len /*= 5*/ )
	{
		DWORD dwback;
		BYTE* jmp = (BYTE*)malloc(len+5);
		VirtualProtect(jmp, len+5, PAGE_EXECUTE_READWRITE, &dwback);
		VirtualProtect(src, len, PAGE_READWRITE, &dwback);
		memcpy(jmp, src, len);
		jmp += len;
		jmp[0] = 0xE9;
		*(DWORD*)(jmp+1) = (DWORD)(src + len - jmp) - 5;
		src[0] = 0xE9;
		*(DWORD*)(src+1) = (DWORD)(dst - src) - 5;
		for (int i = 5; i < len; i++)
		{
			src[i] = 0x90;
		}
		VirtualProtect(src, len, dwback, &dwback);
		return (jmp - len);
	}

	DWORD GetRealDir( HINSTANCE hInstance, wchar_t* Buffer, int MaxSize /*= 512*/ )
	{
		DWORD dwLength = GetModuleFileName(hInstance, Buffer, MaxSize);
		if (dwLength)
		{
			while (dwLength && Buffer[dwLength] != '\\')
			{
				dwLength--;
			}
			if (dwLength)
			{
				Buffer[dwLength] = '\0';
			}
		}
		return dwLength;
	}

	namespace MathHelper
	{
		//************************************
		// Method:    RadToDeg
		// Profit:	  Converts a double Radian to Degree
		// FullName:  SD_Tools::MathHelper::RadToDeg
		// Returns:   double
		// Notes:
		// -None
		//************************************
		double RadToDeg(double Alpha)
		{
			return (Alpha*(180.0/SD_PI));
		}

		//************************************
		// Method:    DegToRad
		// Profit:	  Converts a double Degree to Radian
		// FullName:  SD_Tools::MathHelper::DegToRad
		// Returns:   double
		// Notes:
		// -None
		//************************************
		double DegToRad(double Alpha)
		{
			return  (Alpha/(180.0/SD_PI));
		}

		//************************************
		// Method:    RadToDegf
		// Profit:	  Converts a float Radian to Degree
		// FullName:  SD_Tools::MathHelper::RadToDegf
		// Returns:   float
		// Notes:
		// -None
		//************************************
		float RadToDegf(float Alpha)
		{
			return (Alpha*(180.0f/SD_PI_F));
		}

		//************************************
		// Method:    DegToRadf
		// Profit:	  Converts a float Degree to Radian
		// FullName:  SD_Tools::MathHelper::DegToRadf
		// Returns:   float
		// Notes:
		// -None
		//************************************
		float DegToRadf(float Alpha)
		{
			return (Alpha/(180.0f/SD_PI_F));
		}
	}

	void Vector2::Clear()
	{
		this->x = 0.0f;
		this->y = 0.0f;
	}

	float Vector2::Magnitude()
	{
		return sqrtf(x*x + y*y);
	}

	float Vector2::MagnitudeSqr()
	{
		return (x*x + y*y);
	}

	void Vector2::Subtract( Vector2& Vec )
	{
		this->x -= Vec.x;
		this->y -= Vec.y;
	}

	void Vector2::Add( Vector2& Vec )
	{
		this->x += Vec.x;
		this->y += Vec.y;
	}

	void Vector2::Multiply( float Mul )
	{
		this->x *= Mul;
		this->y *= Mul;
	}

	void Vector2::Turn( float Angle )
	{
		float tx = x;
		float ty = y;
		x = tx*cosf(Angle) - ty*sinf(Angle);
		y = tx*sinf(Angle) + ty*cosf(Angle);
	}

	void Vector2::Turn( float Angle, Vector2& Origin )
	{
		float tx = x-Origin.x;
		float ty = y-Origin.y;
		x = tx*cosf(Angle) - ty*sinf(Angle) + Origin.x;
		y = tx*sinf(Angle) + ty*cosf(Angle) + Origin.y;
	}

	bool Rectangle2::IsPointIn( Vector2& Point )
	{
		return (Point.x >= this->x && Point.x < this->x+this->w && Point.y >= this->y && Point.y < this->y+this->h);
	}
	
	//************************************
	// Method:    Clear
	// Profit:	  Sets the Vector to 0.0f
	// FullName:  SD_Tools::Vector::Clear
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Vector::Clear()
	{
		this->x = 0.0f;
		this->y = 0.0f;
		this->z = 0.0f;
	}

	//************************************
	// Method:    Magnitude
	// Profit:	  Returns the Magnitude(length) of the Vector
	// FullName:  SD_Tools::Vector::Magnitude
	// Returns:   float
	// Notes:
	// -None
	//************************************
	float Vector::Magnitude()
	{
		return sqrtf(x*x + y*y + z*z);
	}

	//************************************
	// Method:    MagnitudeSqr
	// Profit:	  Returns the Square of the Magnitude(length) of the Vector
	// FullName:  SD_Tools::Vector::MagnitudeSqr
	// Returns:   float
	// Notes:
	// -Is faster than the SquareRooted Magnitude and can be used for comparision to other MagnitudeSqr anyway
	//************************************
	float Vector::MagnitudeSqr()
	{
		return (x*x + y*y + z*z);
	}

	//************************************
	// Method:    Subtract
	// Profit:	  Subtracts Vec from this*
	// FullName:  SD_Tools::Vector::Subtract
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Vector::Subtract( Vector& Vec )
	{
		this->x -= Vec.x;
		this->y -= Vec.y;
		this->z -= Vec.z;
	}

	//************************************
	// Method:    Add
	// Profit:	  Adds Vec to this*
	// FullName:  SD_Tools::Vector::Add
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Vector::Add( Vector& Vec )
	{
		this->x += Vec.x;
		this->y += Vec.y;
		this->z += Vec.z;
	}

	//************************************
	// Method:    Multiply
	// Profit:	  Multiplies the vector with a scalar
	// FullName:  SD_Tools::Vector::Multiply
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Vector::Multiply( float Mul )
	{
		this->x *= Mul;
		this->y *= Mul;
		this->z *= Mul;
	}

	//************************************
	// Method:    CrossProduct
	// Profit:	  Crosses this* with Vec
	// FullName:  SD_Tools::Vector::CrossProduct
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Vector::CrossProduct( Vector& Vec )
	{
		this->x = y*Vec.z - z*Vec.y;
		this->y = z*Vec.x - x*Vec.z;
		this->z = x*Vec.y - y*Vec.x;
	}

	//************************************
	// Method:    DotProduct
	// Profit:	  DotProduct of this*Vec
	// FullName:  SD_Tools::Vector::DotProduct
	// Returns:   float
	// Notes:
	// -None
	//************************************
	float Vector::DotProduct( Vector& Vec )
	{
		return (x*Vec.x + y*Vec.y + z*Vec.z);
	}

	//************************************
	// Method:    ToAngles
	// Profit:	  Creates an Angle out of this Vector
	// FullName:  SD_Tools::Vector::ToAngles
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Vector::ToAngles( Angle* Ang )
	{
		float Forward;
		float Yaw, Pitch;

		if (x == 0.0f && y == 0.0f)
		{
			Yaw = 0;
			if (z > 0)
			{
				Pitch = 90.0f;
			}
			else
			{
				Pitch = 270.0f;
			}
		}
		else
		{
			if (x != 0.0f)
			{
				Yaw = MathHelper::RadToDegf(atan2f(y, x));
			}
			else if (y > 0)
			{
				Yaw = 90;
			}
			else
			{
				Yaw = 270;
			}
			if (Yaw < 0)
			{
				Yaw += 360.0f;
			}

			Forward = sqrtf(x*x + y*y);
			Pitch = MathHelper::RadToDegf(atan2f(z, Forward));
			if (Pitch < 0)
			{
				Pitch += 360.0f;
			}
		}

		Ang->Pitch = -Pitch;
		Ang->Yaw = Yaw;
		Ang->Roll = 0.0f;
	}

	void SD_Tools::Vector::Normalize()
	{
		float length = sqrt(x * x + y * y + z * z);
		if(length == 0)
		{
			this->x = 0;
			this->y = 0;
			this->z = 1;
		}
		length = 1 / length;
		this->x = x * length;
		this->y = y * length;
		this->z = z * length;
	}

	//************************************
	// Method:    Clear
	// Profit:	  Sets the Angle to 0.0f
	// FullName:  SD_Tools::Angle::Clear
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Angle::Clear()
	{
		this->x = 0.0f;
		this->y = 0.0f;
		this->z = 0.0f;
	}

	//************************************
	// Method:    Add
	// Profit:	  Adds Ang to this*
	// FullName:  SD_Tools::Angle::Add
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Angle::Add( Angle& Ang )
	{
		this->Pitch += Ang.Pitch;
		this->Yaw += Ang.Yaw;
		this->Roll += Ang.Roll;
	}

	//************************************
	// Method:    Subtract
	// Profit:	  Subtracts Ang from this*
	// FullName:  SD_Tools::Angle::Subtract
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Angle::Subtract( Angle& Ang )
	{
		this->Pitch -= Ang.Pitch;
		this->Yaw -= Ang.Yaw;
		this->Roll -= Ang.Roll;
	}

	//************************************
	// Method:    Multiply
	// Profit:	  Multiplies this with Multiplier
	// FullName:  SD_Tools::Angle::Multiply
	// Returns:   void
	// Notes:
	// -None
	//************************************
	void Angle::Multiply( float Multiplier )
	{
		this->Pitch *= Multiplier;
		this->Yaw *= Multiplier;
		this->Roll *= Multiplier;
	}

	//************************************
	// Method:    ToVectors
	// Profit:	  Transforms the Angle to Forward, Right and Up Vector
	// FullName:  SD_Tools::Angle::ToVectors
	// Returns:   void
	// Notes:
	// -You MUST give all 3 OutVectors - Use ToVectors_s instead if you dont want all
	//************************************
	void Angle::ToVectors( Vector* Forward, Vector* Right, Vector* Up )
	{
		float temp;
		float sr;
		float sp;
		float sy;
		float cr;
		float cp;
		float cy;

		temp = MathHelper::DegToRadf(Yaw);
		sy = sin(temp);
		cy = cos(temp);
		temp = MathHelper::DegToRadf(Pitch);
		sp = sin(temp);
		cp = cos(temp);
		temp = MathHelper::DegToRadf(Roll);
		sr = sin(temp);
		cr = cos(temp);

		Forward->x = cp*cy;
		Forward->y = cp*sy;
		Forward->z = -sp;

		Right->x = (-1*sr*sp*cy+-1*cr*-sy);
		Right->y = (-1*sr*sp*sy+-1*cr*cy);
		Right->z = -1*sr*cp;

		Up->x = (cr*sp*cy+-sr*-sy);
		Up->y = (cr*sp*sy+-sr*cy);
		Up->z = cr*cp;
	}

	//************************************
	// Method:    ToVectors_s
	// Profit:	  Transforms the Angle to Forward, Right and Up Vector
	// FullName:  SD_Tools::Angle::ToVectors_s
	// Returns:   void
	// Notes:
	// -Will accept NULL-Pointers
	//************************************
	void Angle::ToVectors_s( Vector* Forward, Vector* Right, Vector* Up )
	{
		float temp;
		float sr;
		float sp;
		float sy;
		float cr;
		float cp;
		float cy;

		temp = MathHelper::DegToRadf(Yaw);
		sy = sin(temp);
		cy = cos(temp);
		temp = MathHelper::DegToRadf(Pitch);
		sp = sin(temp);
		cp = cos(temp);
		temp = MathHelper::DegToRadf(Roll);
		sr = sin(temp);
		cr = cos(temp);

		if (Forward)
		{
			Forward->x = cp*cy;
			Forward->y = cp*sy;
			Forward->z = -sp;
		}

		if (Right)
		{
			Right->x = (-1*sr*sp*cy+-1*cr*-sy);
			Right->y = (-1*sr*sp*sy+-1*cr*cy);
			Right->z = -1*sr*cp;
		}

		if (Up)
		{
			Up->x = (cr*sp*cy+-sr*-sy);
			Up->y = (cr*sp*sy+-sr*cy);
			Up->z = cr*cp;
		}
	}

	namespace MathHelper
	{
		//************************************
		// Method:    AddVector
		// Profit:	  Adds Vec1 to Vec2
		// FullName:  SD_Tools::MathHelper::AddVector
		// Returns:   SD_Tools::Vector
		// Notes:
		// -None
		//************************************
		SD_Tools::Vector AddVector( Vector& Vec1, Vector& Vec2 )
		{
			return Vector(Vec1.x + Vec2.x, Vec1.y + Vec2.y, Vec1.z + Vec2.z);
		}

		//************************************
		// Method:    SubVector
		// Profit:	  Subtracts Vec2 from Vec1
		// FullName:  SD_Tools::MathHelper::SubVector
		// Returns:   SD_Tools::Vector
		// Notes:
		// -None
		//************************************
		SD_Tools::Vector SubVector( Vector& Vec1, Vector& Vec2 )
		{
			return Vector(Vec1.x - Vec2.x, Vec1.y - Vec2.y, Vec1.z - Vec2.z);
		}

		//************************************
		// Method:    MulVector
		// Profit:	  Multiplies Vec with Mul
		// FullName:  SD_Tools::MathHelper::MulVector
		// Returns:   SD_Tools::Vector
		// Notes:
		// -None
		//************************************
		SD_Tools::Vector MulVector( Vector& Vec, float Mul )
		{
			return Vector(Vec.x * Mul, Vec.y * Mul, Vec.z * Mul);
		}

		//************************************
		// Method:    CrossVector
		// Profit:	  Crosses Vec1 with Vec2
		// FullName:  SD_Tools::MathHelper::CrossVector
		// Returns:   SD_Tools::Vector
		// Notes:
		// -None
		//************************************
		SD_Tools::Vector CrossVector( Vector& Vec1, Vector& Vec2 )
		{
			Vector Temp = Vec1;
			Temp.CrossProduct(Vec2);
			return Temp;
		}

		//************************************
		// Method:    DotVector
		// Profit:	  Vec1 * Vec2
		// FullName:  SD_Tools::MathHelper::DotVector
		// Returns:   float
		// Notes:
		// -None
		//************************************
		float DotVector( Vector& Vec1, Vector& Vec2 )
		{
			return (Vec1.x*Vec2.x + Vec1.y*Vec2.y + Vec1.z*Vec2.z);
		}

		//************************************
		// Method:    ClearVector
		// Profit:	  Clears the Vec
		// FullName:  SD_Tools::MathHelper::ClearVector
		// Returns:   void
		// Notes:
		// -None
		//************************************
		void ClearVector( Vector* Vec )
		{
			Vec->x = 0.0f;
			Vec->y = 0.0f;
			Vec->z = 0.0f;
		}

		//************************************
		// Method:    MagnitudeVector
		// Profit:	  Gets the Magnitude(length) of the given Vector
		// FullName:  SD_Tools::MathHelper::MagnitudeVector
		// Returns:   float
		// Notes:
		// -None
		//************************************
		float MagnitudeVector( Vector& Vec )
		{
			return sqrtf(Vec.x*Vec.x + Vec.y*Vec.y + Vec.z*Vec.z);
		}

		//************************************
		// Method:    MagnitudeSqrVector
		// Profit:	  Gets the Squared Magnitude(length) of the given Vector
		// FullName:  SD_Tools::MathHelper::MagnitudeSqrVector
		// Returns:   float
		// Notes:
		// -Is faster than Magnitude and can be used for comparision with oder MagnitudeSqr Vectors
		//************************************
		float MagnitudeSqrVector( Vector& Vec )
		{
			return (Vec.x*Vec.x + Vec.y*Vec.y + Vec.z*Vec.z);
		}

		//************************************
		// Method:    DistanceVector
		// Profit:	  Returns the Distance between 2 Vectors
		// FullName:  SD_Tools::MathHelper::DistanceVector
		// Returns:   float
		// Notes:
		// -None
		//************************************
		float DistanceVector( Vector& Vec1, Vector& Vec2 )
		{
			return sqrt((Vec2.x-Vec1.x)*(Vec2.x-Vec1.x) + (Vec2.y-Vec1.y)*(Vec2.y-Vec1.y) + (Vec2.z-Vec1.z)*(Vec2.z-Vec1.z));
		}

		//************************************
		// Method:    DistanceSqrVector
		// Profit:	  Returns the Squared Distance between 2 Vectors
		// FullName:  SD_Tools::MathHelper::DistanceSqrVector
		// Returns:   float
		// Notes:
		// -Faster than DistanceVector
		//************************************
		float DistanceSqrVector( Vector& Vec1, Vector& Vec2 )
		{
			return (Vec2.x-Vec1.x)*(Vec2.x-Vec1.x) + (Vec2.y-Vec1.y)*(Vec2.y-Vec1.y) + (Vec2.z-Vec1.z)*(Vec2.z-Vec1.z);
		}

		//************************************
		// Method:    AddAngle
		// Profit:	  Adds 2 Angles
		// FullName:  SD_Tools::MathHelper::AddAngle
		// Returns:   SD_Tools::Angle
		// Notes:
		// -None
		//************************************
		SD_Tools::Angle AddAngle( Angle& Ang1, Angle& Ang2 )
		{
			return Angle(Ang1.Pitch + Ang2.Pitch, Ang1.Yaw + Ang2.Yaw, Ang1.Roll + Ang2.Roll);
		}

		//************************************
		// Method:    SubAngle
		// Profit:	  Subtracts Ang2 from Ang1
		// FullName:  SD_Tools::MathHelper::SubAngle
		// Returns:   SD_Tools::Angle
		// Notes:
		// -None
		//************************************
		SD_Tools::Angle SubAngle( Angle& Ang1, Angle& Ang2 )
		{
			return Angle(Ang1.Pitch - Ang2.Pitch, Ang1.Yaw - Ang2.Yaw, Ang1.Roll - Ang2.Roll);
		}

		//************************************
		// Method:    MulAngle
		// Profit:	  Multiplies Ang with Mul
		// FullName:  SD_Tools::MathHelper::MulAngle
		// Returns:   SD_Tools::Angle
		// Notes:
		// -Null
		//************************************
		SD_Tools::Angle MulAngle( Angle& Ang, float Mul )
		{
			return Angle(Ang.Pitch * Mul, Ang.Yaw * Mul, Ang.Roll * Mul);
		}

		//************************************
		// Method:    ClearAngle
		// Profit:	  Clears the Angle
		// FullName:  SD_Tools::MathHelper::ClearAngle
		// Returns:   void
		// Notes:
		// -None
		//************************************
		void ClearAngle( Angle* Ang )
		{
			Ang->x = 0.0f;
			Ang->y = 0.0f;
			Ang->z = 0.0f;
		}

		//************************************
		// Method:    AngleToVectors
		// Profit:	  Transforms an Angle to Forward, Right and Up Vector
		// FullName:  SD_Tools::MathHelper::AngleToVectors
		// Returns:   void
		// Notes:
		// -DOESN'T accept NULL Vector Pointer
		//************************************
		void AngleToVectors( Angle& Angle, Vector* Forward, Vector* Right, Vector* Up )
		{
			float temp;
			float sr;
			float sp;
			float sy;
			float cr;
			float cp;
			float cy;

			temp = MathHelper::DegToRadf(Angle.Yaw);
			sy = sin(temp);
			cy = cos(temp);
			temp = MathHelper::DegToRadf(Angle.Pitch);
			sp = sin(temp);
			cp = cos(temp);
			temp = MathHelper::DegToRadf(Angle.Roll);
			sr = sin(temp);
			cr = cos(temp);

			Forward->x = cp*cy;
			Forward->y = cp*sy;
			Forward->z = -sp;

			Right->x = (-1*sr*sp*cy+-1*cr*-sy);
			Right->y = (-1*sr*sp*sy+-1*cr*cy);
			Right->z = -1*sr*cp;

			Up->x = (cr*sp*cy+-sr*-sy);
			Up->y = (cr*sp*sy+-sr*cy);
			Up->z = cr*cp;
		}

		//************************************
		// Method:    AngleToVectors_s
		// Profit:	  Transforms an Angle to Forward, Right and Up Vector
		// FullName:  SD_Tools::MathHelper::AngleToVectors_s
		// Returns:   void
		// Notes:
		// -May pass Null-Pointers
		//************************************
		void AngleToVectors_s( Angle& Angle, Vector* Forward, Vector* Right, Vector* Up )
		{
			float temp;
			float sr;
			float sp;
			float sy;
			float cr;
			float cp;
			float cy;

			temp = MathHelper::DegToRadf(Angle.Yaw);
			sy = sin(temp);
			cy = cos(temp);
			temp = MathHelper::DegToRadf(Angle.Pitch);
			sp = sin(temp);
			cp = cos(temp);
			temp = MathHelper::DegToRadf(Angle.Roll);
			sr = sin(temp);
			cr = cos(temp);

			if (Forward)
			{
				Forward->x = cp*cy;
				Forward->y = cp*sy;
				Forward->z = -sp;
			}

			if (Right)
			{
				Right->x = (-1*sr*sp*cy+-1*cr*-sy);
				Right->y = (-1*sr*sp*sy+-1*cr*cy);
				Right->z = -1*sr*cp;
			}

			if (Up)
			{
				Up->x = (cr*sp*cy+-sr*-sy);
				Up->y = (cr*sp*sy+-sr*cy);
				Up->z = cr*cp;
			}
		}

		//************************************
		// Method:    VectorToAngle
		// Profit:	  Converts a Vector to an Angle
		// FullName:  SD_Tools::MathHelper::VectorToAngle
		// Returns:   void
		// Notes:
		// -None
		//************************************
		void VectorToAngle( Vector& Vector, Angle* Out )
		{
			float Forward;
			float Yaw, Pitch;

			if (Vector.x == 0.0f && Vector.y == 0.0f)
			{
				Yaw = 0;
				if (Vector.z > 0)
				{
					Pitch = 90.0f;
				}
				else
				{
					Pitch = 270.0f;
				}
			}
			else
			{
				if (Vector.x != 0.0f)
				{
					Yaw = MathHelper::RadToDegf(atan2f(Vector.y, Vector.x));
				}
				else if (Vector.y > 0)
				{
					Yaw = 90;
				}
				else
				{
					Yaw = 270;
				}
				if (Yaw < 0)
				{
					Yaw += 360.0f;
				}

				Forward = sqrtf(Vector.x*Vector.x + Vector.y*Vector.y);
				Pitch = MathHelper::RadToDegf(atan2f(Vector.z, Forward));
				if (Pitch < 0)
				{
					Pitch += 360.0f;
				}
			}

			Out->Pitch = -Pitch;
			Out->Yaw = Yaw;
			Out->Roll = 0.0f;
		}


	}

}

